#include <PID_v1.h>

#define LEFT_SENSOR 14
#define CENTER_SENSOR 16
#define RIGHT_SENSOR 18

#define MOTOR_A1 5
#define MOTOR_A2 6
#define MOTOR_B1 9
#define MOTOR_B2 10

double Setpoint = 0; // 目标值为0，表示车辆应当保持在中心位置
double Kp = 2;
double Ki = 5;
double Kd = 1;

int s;
int pin[3] = {A4, A2, A0};  

double Input, Output;
PID pid(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

int leftSensorValue, centerSensorValue, rightSensorValue;

void setup() {
  Serial.begin(9600);
  pinMode(MOTOR_A1, OUTPUT);
  pinMode(MOTOR_A2, OUTPUT);
  pinMode(MOTOR_B1, OUTPUT);
  pinMode(MOTOR_B2, OUTPUT);
  pinMode(CENTER_SENSOR, INPUT); // 设置中心传感器为输入模式

  pid.SetMode(AUTOMATIC);
  pid.SetOutputLimits(-255, 255);

  Serial.println("PID Tuning:");
  Serial.print("Kp: ");
  Serial.println(Kp);
  Serial.print("Ki: ");
  Serial.println(Ki);
  Serial.print("Kd: ");
  Serial.println(Kd);
      // 初始化串行通信
  Serial.begin(9600);
}

void loop() { 
//  leftSensorValue = analogRead(LEFT_SENSOR);
//  rightSensorValue = analogRead(RIGHT_SENSOR);
//  
//  // 使用数字输入来检测中心传感器是否检测到黑色
//  centerSensorValue = digitalRead(CENTER_SENSOR); 
//  leftSensorValue = digitalRead(LEFT_SENSOR); 
//  rightSensorValue = digitalRead(RIGHT_SENSOR); 

  // 计算传感器读数的误差，以便作为PID控制器的输入
  double error = (double)rightSensorValue - (double)leftSensorValue;
  Input = error / 1023.0;

  pid.Compute();

  // 使用PID输出来控制电机转向幅度
  int leftSpeed = constrain(200 + Output, 0, 255); // 左轮速度
  int rightSpeed = constrain(200 - Output, 0, 255); // 右轮速度

  // 默认直行
  int motorA1Value = leftSpeed;
  int motorA2Value = LOW;
  int motorB1Value = rightSpeed;
  int motorB2Value = LOW;


switch (s) 
        {
          case 0x00: //三个均未触发
          {  // 默认直行
  int motorA1Value = leftSpeed;
  int motorA2Value = LOW;
  int motorB1Value = rightSpeed;
  int motorB2Value = LOW;}
          break;
          case 0x01: //右侧传感器触发，直线上摆动或遇到右转弯
          {
                motorA1Value = leftSpeed;
                motorA2Value = LOW;
                motorB1Value = LOW;
                motorB2Value = rightSpeed;
            }
          break;
          case 0x02: //中间传感器触发，直线上直行
          {
              // 默认直行
  int motorA1Value = leftSpeed;
  int motorA2Value = LOW;
  int motorB1Value = rightSpeed;
  int motorB2Value = LOW;
            }
          break;
          case 0x03: //右侧两个触发，遇到右转弯
          {motorA1Value = leftSpeed;
    motorA2Value = LOW;
    motorB1Value = LOW;
    motorB2Value = rightSpeed;}
          break;
          case 0x04: //左侧传感器触发，直线上摆动或遇到左转弯
          {
            motorA1Value = LOW;
    motorA2Value = leftSpeed;
    motorB1Value = rightSpeed;
    motorB2Value = LOW;}
          break;
          case 0x06: //左侧两个触发，遇到左转弯
          {motorA1Value = LOW;
    motorA2Value = leftSpeed;
    motorB1Value = rightSpeed;
    motorB2Value = LOW;}
          break;
          case 0x07: //三个同时触发，遇到十字路口
          {  // 默认直行
  int motorA1Value = leftSpeed;
  int motorA2Value = LOW;
  int motorB1Value = rightSpeed;
  int motorB2Value = LOW;}
          break;
          default:;break;
        }


  // 如果中心传感器检测到黑色，则执行转弯动作

  if (centerSensorValue == LOW) {
    // 左转
    motorA1Value = LOW;
    motorA2Value = leftSpeed;
    motorB1Value = rightSpeed;
    motorB2Value = LOW;
  }

  // 如果左侧传感器检测到黑色，则右转
  if (leftSensorValue < 500) {
    x = leftSpeed;
    motorA2Value = LOW;
    motorB1Value = LOW;
    motorB2Value = rightSpeed;
  }

  // 如果右侧传感器检测到黑色，则左转
  if (rightSensorValue < 500) {
    motorA1Value = LOW;
    motorA2Value = leftSpeed;
    motorB1Value = rightSpeed;
    motorB2Value = LOW;
  }

  // 控制电机转向
  analogWrite(MOTOR_A1, motorA1Value);
  digitalWrite(MOTOR_A2, motorA2Value);
  analogWrite(MOTOR_B1, motorB1Value);
  digitalWrite(MOTOR_B2, motorB2Value);

  // 输出调试信息
  Serial.print("Input: ");
  Serial.print(Input);
  Serial.print(" Output: ");
  Serial.println(Output);

  delay(10);
}
